#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <cmath>
#include <vector>
#include <zlib.h>  // For CRC32 checksum

class NavSimulate {
public:
    NavSimulate() : nh_("~") {
        // 初始化参数
        nh_.param("initial_x", x_, 0.0);
        nh_.param("initial_y", y_, 0.0);
        nh_.param("initial_yaw", yaw_, 0.0);
        
        // 初始化变量
        linear_velocity_ = 0.0;
        angular_velocity_ = 0.0;
        map_feature_ = 0;
        
        // 订阅者和发布者
        cmd_vel_sub_ = nh_.subscribe("/cmd_vel", 10, &NavSimulate::cmdVelCallback, this);
        map_sub_ = nh_.subscribe("/map", 1, &NavSimulate::mapCallback, this);
        initialpose_sub_ = nh_.subscribe("/initialpose", 10, &NavSimulate::poseResetCallback, this);
        local_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/local_map", 10);
        
        // 定时器 (20Hz)
        timer_ = nh_.createTimer(ros::Duration(1.0 / 20.0), &NavSimulate::timerCallback, this);
        last_time_ = ros::Time::now();
    }

private:
    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
        linear_velocity_ = msg->linear.x;
        angular_velocity_ = msg->angular.z;
    }

    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
        // 计算新地图的 CRC32 校验和
        uLong new_feature = crc32(0L, Z_NULL, 0);
        const Bytef* data_ptr = reinterpret_cast<const Bytef*>(msg->data.data());
        uInt data_len = msg->data.size() * sizeof(int8_t);
        new_feature = crc32(new_feature, data_ptr, data_len);
        
        // 添加地图元信息到校验和
        new_feature = crc32(new_feature, reinterpret_cast<const Bytef*>(&msg->info.width), sizeof(int));
        new_feature = crc32(new_feature, reinterpret_cast<const Bytef*>(&msg->info.height), sizeof(int));
        
        // 只有当特征值变化时才更新地图
        if (map_feature_ != new_feature) {
            global_map_ = *msg;
            map_feature_ = new_feature;
            ROS_DEBUG("Global map updated with new feature: %lu", new_feature);
        } else {
            ROS_DEBUG("Received duplicate map, skipping update");
        }
    }

    void timerCallback(const ros::TimerEvent& event) {
        ros::Time current_time = ros::Time::now();
        double dt = (current_time - last_time_).toSec();
        
        if (dt > 0) {
            // 更新位姿
            x_ += linear_velocity_ * std::cos(yaw_) * dt;
            y_ += linear_velocity_ * std::sin(yaw_) * dt;
            yaw_ += angular_velocity_ * dt;
        }
        last_time_ = current_time;
        
        // 发布 TF 变换
        publishTransform();
        
        // 发布局部地图
        if (global_map_.data.size() > 0) {
            nav_msgs::OccupancyGrid local_map = createLocalMap();
            local_map_pub_.publish(local_map);
        }
    }

    void publishTransform() {
        geometry_msgs::TransformStamped transform;
        transform.header.stamp = ros::Time::now();
        transform.header.frame_id = "map";
        transform.child_frame_id = "body";
        
        transform.transform.translation.x = x_;
        transform.transform.translation.y = y_;
        transform.transform.translation.z = 0.0;
        
        tf2::Quaternion q;
        q.setRPY(0, 0, yaw_);
        transform.transform.rotation.x = q.x();
        transform.transform.rotation.y = q.y();
        transform.transform.rotation.z = q.z();
        transform.transform.rotation.w = q.w();
        
        tf_broadcaster_.sendTransform(transform);
    }

    nav_msgs::OccupancyGrid createLocalMap() {
        nav_msgs::OccupancyGrid local_map;
        const auto& info = global_map_.info;
        const double resolution = info.resolution;
        
        // 设置局部地图参数
        const int width = std::max(1, static_cast<int>(12.0 / resolution));
        const int height = std::max(1, static_cast<int>(12.0 / resolution));
        
        local_map.header.stamp = ros::Time::now();
        local_map.header.frame_id = "body";
        local_map.info.resolution = resolution;
        local_map.info.width = width;
        local_map.info.height = height;
        local_map.info.origin.position.x = -4.0;
        local_map.info.origin.position.y = -4.0;
        local_map.info.origin.orientation.w = 1.0;  // 单位旋转
        
        local_map.data.resize(width * height);
        
        const double cos_yaw = std::cos(yaw_);
        const double sin_yaw = std::sin(yaw_);
        const double origin_x = info.origin.position.x;
        const double origin_y = info.origin.position.y;
        const int map_width = info.width;
        const int map_height = info.height;
        
        for (int row = 0; row < height; ++row) {
            for (int col = 0; col < width; ++col) {
                // body 坐标系下的坐标
                const double x_prime = -4.0 + (col + 0.5) * resolution;
                const double y_prime = -4.0 + (row + 0.5) * resolution;
                
                // 转换到 map 坐标系
                const double x_map = x_ + x_prime * cos_yaw - y_prime * sin_yaw;
                const double y_map = y_ + x_prime * sin_yaw + y_prime * cos_yaw;
                
                // 转换到全局地图的 cell 坐标
                const double cell_x = (x_map - origin_x) / resolution;
                const double cell_y = (y_map - origin_y) / resolution;
                
                const int cell_col = static_cast<int>(std::floor(cell_x));
                const int cell_row = static_cast<int>(std::floor(cell_y));
                
                // 检查是否在地图范围内
                int8_t value = -1;  // 默认未知
                if (cell_col >= 0 && cell_col < map_width && 
                    cell_row >= 0 && cell_row < map_height) {
                    const int index = cell_row * map_width + cell_col;
                    value = global_map_.data[index];
                }
                
                local_map.data[row * width + col] = value;
            }
        }
        
        return local_map;
    }

    void poseResetCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
        // 提取位置信息
        x_ = msg->pose.pose.position.x;
        y_ = msg->pose.pose.position.y;
        
        // 提取姿态信息并转换为欧拉角
        const auto& orientation = msg->pose.pose.orientation;
        tf2::Quaternion q(
            orientation.x,
            orientation.y,
            orientation.z,
            orientation.w
        );
        tf2::Matrix3x3 m(q);
        double roll, pitch;
        m.getRPY(roll, pitch, yaw_);
        
        // 更新时间戳以避免 dt 异常
        last_time_ = ros::Time::now();
    }

    // ROS 相关
    ros::NodeHandle nh_;
    ros::Subscriber cmd_vel_sub_;
    ros::Subscriber map_sub_;
    ros::Subscriber initialpose_sub_;
    ros::Publisher local_map_pub_;
    ros::Timer timer_;
    tf2_ros::TransformBroadcaster tf_broadcaster_;
    
    // 状态变量
    double x_, y_, yaw_;
    double linear_velocity_;
    double angular_velocity_;
    nav_msgs::OccupancyGrid global_map_;
    uLong map_feature_;  // 地图特征值 (CRC32)
    ros::Time last_time_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "nav_simulate");
    NavSimulate nav_simulate;
    ros::spin();
    return 0;
}